#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_control/action/move_action.hpp"
#include <chrono>
#include <functional>
#include <memory>

using namespace std::chrono_literals;
using MoveAction = robot_control::action::MoveAction;
using GoalHandleMoveAction = rclcpp_action::ClientGoalHandle<MoveAction>;

class MoveActionClient : public rclcpp::Node
{
public:
    MoveActionClient() : Node("move_action_client")
    {
        action_client_ = rclcpp_action::create_client<MoveAction>(this, "move_to_goal");
        RCLCPP_INFO(this->get_logger(), "移动动作客户端已启动");
    }
    
    void send_goal(float target_x, float target_y)
    {
        // 等待动作服务器
        if (!action_client_->wait_for_action_server(5s)) {
            RCLCPP_ERROR(this->get_logger(), "动作服务器不可用");
            return;
        }
        
        auto goal_msg = MoveAction::Goal();
        goal_msg.target_x = target_x;
        goal_msg.target_y = target_y;
        
        RCLCPP_INFO(this->get_logger(), "发送移动目标: (%.2f, %.2f)", target_x, target_y);
        
        // 设置目标选项 - 修复回调函数签名
        auto send_goal_options = rclcpp_action::Client<MoveAction>::SendGoalOptions();
        
        // 修正：使用正确的回调函数签名
        send_goal_options.goal_response_callback =
            [this](GoalHandleMoveAction::SharedPtr goal_handle) {
                this->goal_response_callback(goal_handle);
            };
            
        send_goal_options.feedback_callback =
            [this](GoalHandleMoveAction::SharedPtr goal_handle,
                   const std::shared_ptr<const MoveAction::Feedback> feedback) {
                this->feedback_callback(goal_handle, feedback);
            };
            
        send_goal_options.result_callback =
            [this](const GoalHandleMoveAction::WrappedResult &result) {
                this->result_callback(result);
            };
        
        // 发送目标
        action_client_->async_send_goal(goal_msg, send_goal_options);
    }

private:
    rclcpp_action::Client<MoveAction>::SharedPtr action_client_;
    
    // 修正：使用正确的参数类型
    void goal_response_callback(GoalHandleMoveAction::SharedPtr goal_handle)
    {
        if (!goal_handle) {
            RCLCPP_ERROR(this->get_logger(), "目标被拒绝");
        } else {
            RCLCPP_INFO(this->get_logger(), "目标已被接受，开始执行");
        }
    }
    
    void feedback_callback(
        GoalHandleMoveAction::SharedPtr,
        const std::shared_ptr<const MoveAction::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "移动反馈: 当前位置(%.2f, %.2f), 剩余距离: %.2f",
                   feedback->current_x, feedback->current_y, feedback->remaining_distance);
    }
    
    void result_callback(const GoalHandleMoveAction::WrappedResult &result)
    {
        switch (result.code) {
            case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO(this->get_logger(), "移动成功! 最终位置: (%.2f, %.2f)",
                           result.result->final_x, result.result->final_y);
                break;
            case rclcpp_action::ResultCode::ABORTED:
                RCLCPP_ERROR(this->get_logger(), "移动被中止");
                break;
            case rclcpp_action::ResultCode::CANCELED:
                RCLCPP_WARN(this->get_logger(), "移动被取消");
                break;
            default:
                RCLCPP_ERROR(this->get_logger(), "未知结果");
                break;
        }
        
        rclcpp::shutdown();
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    
    if (argc != 3) {
        RCLCPP_ERROR(rclcpp::get_logger("move_action_client"), "用法: move_action_client <x> <y>");
        RCLCPP_ERROR(rclcpp::get_logger("move_action_client"), "示例: move_action_client 1.5 2.0");
        return 1;
    }
    
    float target_x = std::stof(argv[1]);
    float target_y = std::stof(argv[2]);
    
    auto client = std::make_shared<MoveActionClient>();
    client->send_goal(target_x, target_y);
    
    rclcpp::spin(client);
    rclcpp::shutdown();
    return 0;
}